//just template store code, not complete

void lineColor()
{
	const int line_max = 7;
	static const CvScalar line_colors[line_max] =
	{
		{{0,0,255}},
		{{0,128,255}},
		{{0,200,200}},
		{{0,255,0}},
		{{200,200,0}},
		{{255,0,0}},
		{{255,0,255}}
	};

	for( y = 0, i = 0; y < pattern_size.height; y++ )
	{
		CvScalar color = line_colors[y % line_max];
	}

}


static void normaLizePoints(const vector<Point2f> &pt, vector<Point2f> &pt_norm, Mat &T, Mat &S)
{
	//averate mx, my
	pt_norm.resize(pt.size());
	float mx = 0.0, my = 0.0;
	for (int i = 0; i<pt.size(); ++i)
	{
		mx += pt[i].x;
		my += pt[i].y;
	}
	mx /= pt.size();
	my /= pt.size();
	for (int i = 0; i<pt.size(); ++i)
	{
		pt_norm[i].x = pt[i].x - mx;
		pt_norm[i].y = pt[i].y - my;
	}

	T = Mat::eye(3, 3, CV_64FC1);
	T.at<double>(0, 2) = -mx;
	T.at<double>(1, 2) = -my;

	//distance d
	float d = 0.0;
	for (int i = 0; i<pt_norm.size(); ++i)
	{
		d += sqrt(pt_norm[i].x * pt_norm[i].x + pt_norm[i].y * pt_norm[i].y);
	}
	d /= pt_norm.size();

	for (int i = 0; i<pt_norm.size(); ++i)
	{
		pt_norm[i].x *= 1.4142/d;
		pt_norm[i].y *= 1.4142/d;
	}

	S = Mat::eye(3, 3, CV_64FC1);
	S.at<double>(0, 0) = 1.4142/d;
	S.at<double>(1, 1) = 1.4142/d;

	CvxMat_d t_cvxmat(&T);
	CvxMat_d s_cvxmat(&S);

	int a = 1;

}

static void normalizeData(CvMat *normMat, Mat &T, Mat &S)
{
	// the result may not correct
	Scalar avg_val = cvAvg(normMat);
	//move to data center
	cvSubS(normMat, avg_val, normMat);

	T = Mat::eye(3, 3, CV_64FC1);
	T.at<double>(0, 2) = -avg_val.val[0];
	T.at<double>(1, 2) = -avg_val.val[1];

	// scale sqrt(2)/d
	CvMat* xnormMat = cvCreateMat( 1, normMat->cols, CV_32FC1);
	CvMat* ynormMat = cvCreateMat( 1, normMat->cols, CV_32FC1);

	int ch = CV_MAT_CN(normMat->type);

	int type = CV_MAT_TYPE(ynormMat->type);
	int _32fc1 = CV_32FC1;
	int _64fc1 = CV_64FC1;
	cvSplit(normMat, xnormMat, ynormMat, NULL, NULL);

	cvMul(xnormMat, xnormMat, xnormMat);  // x * x
	cvMul(ynormMat, ynormMat, ynormMat);  // y * y
	cvAdd(xnormMat, ynormMat, ynormMat);  // x*x + y*y

	double d = 0.0;
	for (int i = 0; i<ynormMat->cols; ++i)
	{
		float *data = (ynormMat->data.fl + i);
		d += sqrt(data[0]);
	}
	d /= ynormMat->cols;

	cvScale(normMat, normMat, CV_SQRT2/d);

	S = Mat::eye(3, 3, CV_64FC1);
	S.at<double>(0, 0) = CV_SQRT2/d;
	S.at<double>(1, 1) = CV_SQRT2/d;

	cvReleaseMat(&xnormMat);
	cvReleaseMat(&ynormMat);
}

#if 0
int main()
{

	vector<string> fileNames;
	vector<IplImage *> imageVec;
	bool isOk = CvxIo::travel_dir("save_image\\15_9\\top\\960\\*.png", fileNames);

	for (int i = 0; i<fileNames.size(); ++i)
	{
		string fname = string("save_image\\15_9\\top\\960\\") + fileNames[i];
		IplImage *image = cvLoadImage(fname.c_str(), 1);
		assert(image);
		imageVec.push_back(image);
	}

	//load checkboard corner points
	CChessBoard board(cvSize(15, 9), 25);

	FILE *pf = fopen("save_image\\15_9\\top\\960\\points.txt", "r");
	vector<int> idx;
	vector<vector<Point2f> > chessboardPts;
	CvSize size;
	CvxSF::loadChessboard(pf, idx, chessboardPts, size);
	fclose(pf);


	vector<vector<Point3f> > objPts;

	bool camputerExtrinsic;
	Mat cameraMatrix;
	Mat distCoeffs;
	vector<Mat> rvecs;
	vector<Mat> tvecs;
	bool computerExtrinsic;

	/* Finds perspective transformation between the object plane and image (view) plane */
	// 	CVAPI(int) cvFindHomography( const CvMat* src_points,
	// 		const CvMat* dst_points,
	// 		CvMat* homography,
	// 		int method CV_DEFAULT(0),
	// 		double ransacReprojThreshold CV_DEFAULT(3),
	// 		CvMat* mask CV_DEFAULT(0));

	//using cv::findHomography
	{

		Mat H = cv::findHomography(Mat(board._pts2d), Mat(chessboardPts[0]));

		printf("using cv::findHomography: \n");	
		for (int i = 0; i<3; ++i)
		{
			for (int j = 0; j<3; ++j)
			{
				printf("%f ", H.at<double>(i, j));
			}
			printf("\n");
		}
	}
	printf("\n");

	if(0)
	{
		vector<Point2f> obj_norm;
		vector<Point2f> img_norm;
		Mat Sobj, Tobj;
		Mat Simg, Timg;
		normaLizePoints(board._pts2d, obj_norm, Sobj, Tobj);
		normaLizePoints(chessboardPts[0], img_norm, Simg, Timg);

		Mat H = cv::findHomography(Mat(obj_norm), Mat(img_norm));

		//H = Tobj.t()*Sobj*H*Simg*Timg;
		H = (Timg*Simg).inv()*H*(Tobj*Sobj);

		H *= 1.0/H.at<double>(2, 2);

		double H_vev[9];
		printf("using cv::findHomography: \n");	
		for (int i = 0; i<3; ++i)
		{
			for (int j = 0; j<3; ++j)
			{
				printf("%f ", H.at<double>(i, j));
				H_vev[3*i+j] =  H.at<double>(i, j);
			}
			printf("\n");
		}

		//test homography
		for (int i = 0; i<board._pts2d.size(); ++i)
		{
			float d1 = board._pts3d[i].x;
			float d2 = board._pts3d[i].y;
			float d3 = 1.0;
			float x = H_vev[0] * d1 + H_vev[1] * d2 + H_vev[2] * d3;
			float y = H_vev[3] * d1 + H_vev[4] * d2 + H_vev[5] * d3;
			float z = H_vev[6] * d1 + H_vev[7] * d2 + H_vev[8] * d3;

			if(z != 0.0)
			{
				x /= z;
				y /= z;
			}

			float xx = chessboardPts[0][i].x;
			float yy = chessboardPts[0][i].y;
			//		printf("%f %f\n", x/xx, y/yy);
		}
	}

	{

		Mat objMat = Mat(board._pts3d, true);
		Mat imgMat = Mat(chessboardPts[0], true);

		CvMat obj_norm = objMat;
		CvMat img_norm = imgMat;

		int cobj = CV_MAT_CN(obj_norm.type);
		int cimg = CV_MAT_CN(img_norm.type);

		Mat Sobj, Tobj;
		Mat Simg, Timg;

		cvReshape(&obj_norm, &obj_norm, 3, 1);
		cvReshape(&img_norm, &img_norm, 2, 1);

		normalizeData(&obj_norm, Sobj, Tobj);
		normalizeData(&img_norm, Simg, Timg);

		Mat H = cvCreateMat(3, 3, CV_64FC1);
		CvMat H_mat = H;

		cvFindHomography(&obj_norm, &img_norm, &H_mat);

		H = (Timg*Simg).inv()*H*(Tobj*Sobj);

		H *= 1.0/H.at<double>(2, 2);

		double H_vev[9];
		printf("using cv::findHomography: \n");	
		for (int i = 0; i<3; ++i)
		{
			for (int j = 0; j<3; ++j)
			{
				printf("%f ", H.at<double>(i, j));
				H_vev[3*i+j] =  H.at<double>(i, j);
			}
			printf("\n");
		}

		//test homography
		for (int i = 0; i<board._pts2d.size(); ++i)
		{
			float d1 = board._pts3d[i].x;
			float d2 = board._pts3d[i].y;
			float d3 = 1.0;
			float x = H_vev[0] * d1 + H_vev[1] * d2 + H_vev[2] * d3;
			float y = H_vev[3] * d1 + H_vev[4] * d2 + H_vev[5] * d3;
			float z = H_vev[6] * d1 + H_vev[7] * d2 + H_vev[8] * d3;

			if(z != 0.0)
			{
				x /= z;
				y /= z;
			}

			float xx = chessboardPts[0][i].x;
			float yy = chessboardPts[0][i].y;
			//		printf("%f %f\n", x/xx, y/yy);
		}
		/*
		*/
	}

	return 1;


	//using cvFindHomography
	{
		CvMat *objPtsMat = cvCreateMat(1, 15*9, CV_32FC3);
		for (int i = 0; i< board._pts3d.size(); ++i)
		{
			float *data = (float*)(objPtsMat->data.fl + 3*i);
			data[0] = board._pts3d[i].x;
			data[1] = board._pts3d[i].y;
			data[2] = 1.0;
		}

		CvMat *imgPts = cvCreateMat(1, 15*9, CV_32FC3);
		CvMat *H = cvCreateMat(3, 3, CV_32FC1);

		for (int j = 0; j<board._pts3d.size(); ++j)
		{
			float *data = (float*)(imgPts->data.fl + j * 3);
			data[0] = chessboardPts[0][j].x;
			data[1] = chessboardPts[0][j].y;
			data[2] = 1.0;
		}
		cvFindHomography(objPtsMat, imgPts, H);

		// find H
		float H_vev[9];
		printf("using cvFindHomography:\n");
		for (int i = 0; i<3; ++i)
		{
			float *data = (float*)(H->data.ptr + i * H->step);
			printf("%f %f %f\n", data[0], data[1], data[2]);
			H_vev[i*3] = data[0];
			H_vev[i*3+1] = data[1];
			H_vev[i*3+2] = data[2];
		}


		//m = HM
		for (int i = 0; i< board._pts3d.size(); ++i)
		{
			float d1 = board._pts3d[i].x;
			float d2 = board._pts3d[i].y;
			float d3 = 1.0;
			float x = H_vev[0] * d1 + H_vev[1] * d2 + H_vev[2] * d3;
			float y = H_vev[3] * d1 + H_vev[4] * d2 + H_vev[5] * d3;
			float z = H_vev[6] * d1 + H_vev[7] * d2 + H_vev[8] * d3;

			if(z != 0.0)
			{
				x /= z;
				y /= z;
			}

			float xx = chessboardPts[0][i].x;
			float yy = chessboardPts[0][i].y;
			//printf("%f %f\n", x/xx, y/yy);
		}

	}
	printf("\n");

	printf("after normalization:\n");
	/************************************************************************/
	/* 
	ax = m(1,:);
	ay = m(2,:);

	mxx = mean(ax);
	myy = mean(ay);
	ax = ax - mxx;
	ay = ay - myy;

	scxx = mean(abs(ax));
	scyy = mean(abs(ay));


	Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1];
	inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1];

	mn = Hnorm*m;
	*/
	/************************************************************************/
	//using cvFindHomography 
	{
		CvMat *objPtsMat = cvCreateMat(15*9, 3, CV_32FC1);
		for (int i = 0; i<board._pts3d.size(); ++i)
		{
			float *data = (float*)(objPtsMat->data.ptr + i * objPtsMat->step);
			data[0] = board._pts3d[i].x;
			data[1] = board._pts3d[i].y;
			data[2] = 1.0;
		}

		CvMat *imgPts = cvCreateMat(15*9, 3, CV_32FC1);
		CvMat *H = cvCreateMat(3, 3, CV_32FC1);

		for (int j = 0; j<chessboardPts[0].size(); ++j)
		{
			float *data = (float*)(imgPts->data.ptr + j * imgPts->step);
			data[0] = chessboardPts[0][j].x;
			data[1] = chessboardPts[0][j].y;
			data[2] = 1.0;
		}
		CvMat * Hnorm = cvCreateMat(3, 3, CV_32FC1);
		CvMat * inv_Hnorm = cvCreateMat(3, 3, CV_32FC1);
		CvMat * Hnorm2 = cvCreateMat(3, 3, CV_32FC1);
		CvMat * inv_Hnorm2 = cvCreateMat(3, 3, CV_32FC1);

		normMat(objPtsMat, Hnorm, inv_Hnorm);
		normMat(imgPts, Hnorm2, inv_Hnorm2);

		cvFindHomography(objPtsMat, imgPts, H);

		cvMatMul(inv_Hnorm, H, H);
		cvMatMul(H, Hnorm2, H);


		double s = 1.0/cvmGet(H, 2, 2);
		cvScale(H, H, s);

		// find H
		float H_vev[9];
		printf("using cvFindHomography:\n");
		for (int i = 0; i<3; ++i)
		{
			float *data = (float*)(H->data.ptr + i * H->step);
			printf("%f %f %f\n", data[0], data[1], data[2]);
			H_vev[i*3] = data[0];
			H_vev[i*3+1] = data[1];
			H_vev[i*3+2] = data[2];
		}

		//m = HM
		for (int i = 0; i< board._pts3d.size(); ++i)
		{
			float d1 = board._pts3d[i].x;
			float d2 = board._pts3d[i].y;
			float d3 = 1.0;
			float x = H_vev[0] * d1 + H_vev[1] * d2 + H_vev[2] * d3;
			float y = H_vev[3] * d1 + H_vev[4] * d2 + H_vev[5] * d3;
			float z = H_vev[6] * d1 + H_vev[7] * d2 + H_vev[8] * d3;

			if(z != 0.0)
			{
				x /= z;
				y /= z;
			}

			float xx = chessboardPts[0][i].x;
			float yy = chessboardPts[0][i].y;
			//		printf("%f %f\n", x/xx, y/yy);
		}

	}

	//	CvxCalib3d::closedFormSolution(board._pts3d, chessboardPts, size, cameraMatrix, distCoeffs, rvecs, tvecs, true);	
	cvWaitKey(0);
}
#endif